﻿using System;


namespace serial
{

    public class szyProtocol
    {


        #region 接收部分
        private static float pitch;
        private static float roll;
        private static float yaw;
        private static float compass;
        private static float temp;
        private static float depth;
        private static float electric;
        private byte[] serialdata;

        public static float Pitch { get => pitch; set => pitch = value; }
        public static float Roll { get => roll; set => roll = value; }
        public static float Yaw { get => yaw; set => yaw = value; }
        public static float Compass { get => compass; set => compass = value; }
        public static float Temp { get => temp; set => temp = value; }
        public static float Depth { get => depth; set => depth = value; }
        public static float Electric { get => electric; set => electric = value; }
        public byte[] Serialdata { get => serialdata; set => serialdata = value; }

        public static float sendpitch()
        {
            return Pitch;
        }
        public static float sendroll()
        {
            return Roll;
        }
        public static float sendyaw()
        {
            return Yaw;
        }
        public static float sendcompass()
        {
            return Compass;
        }
        public static float sendtemp()
        {
            return Temp;
        }
        public static float senddepth()
        {
            return Depth;
        }
        public static float sendelec()
        {
            return Electric;
        }
        byte[] buf;
        public void CheckData()
        {
            while (true)
            {
                buf = new byte[Serialdata.Length];
                buf = Serialdata;
                if (buf.Length == 8)
                {
                    if (buf[0] == 0x02)
                    {
                        switch (buf[1])
                        {
                            case 0x41: Pitch = BitConverter.ToSingle(buf, 2);  break;
                            case 0x42: Roll = BitConverter.ToSingle(buf, 2);  break;
                            case 0x43: Yaw = BitConverter.ToSingle(buf, 2);  break;
                            case 0x44: Compass = BitConverter.ToSingle(buf, 2);break;
                            case 0x45: Temp = BitConverter.ToSingle(buf, 2); break;
                            case 0x46: Depth = BitConverter.ToSingle(buf, 2);  break;
                            case 0x47: Electric = BitConverter.ToSingle(buf, 2);  break;
                            default:
                                break;
                        }
                    }

                }
                else if(buf.Length%8==0)
                {
                    for (int i = 0; i < buf.Length; i += 8)
                    {
                        if (buf[i] == 0x02)
                        {
                            switch (buf[i + 1])
                            {
                                case 0x41: Pitch = BitConverter.ToSingle(buf, 2); break;
                                case 0x42: Roll = BitConverter.ToSingle(buf, 2); break;
                                case 0x43: Yaw = BitConverter.ToSingle(buf, 2); break;
                                case 0x44: Compass = BitConverter.ToSingle(buf, 2); break;
                                case 0x45: Temp = BitConverter.ToSingle(buf, 2); break;
                                case 0x46: Depth = BitConverter.ToSingle(buf, 2); break;
                                case 0x47: Electric = BitConverter.ToSingle(buf, 2); break;
                                default:
                                    break;
                            }
                        }
                    }

                }
            }
        }



        #endregion

        #region   发送部分
        private int forwardThrottle;            //前向油门
        private int transverseThrottle;       //横向油门
        private int verticalThorttle;            //垂向油门
        private int lightPwm;                      //灯
        private int ptz;                                //云台
        private int steering;                       //舵机
        private float Ppitch;                       //调参使用
        private float Proll;
        private float Pyaw;
        private float Ipitch;
        private float Iroll;
        private float Iyaw;
        private float Dpitch;
        private float Droll;
        private float Dyaw;

        byte[] moveData = new byte[28];
        byte[] adjustData = new byte[40];
        public int ForwardThrottle { get => forwardThrottle; set => forwardThrottle = value; }
        public int TransverseThrottle { get => transverseThrottle; set => transverseThrottle = value; }
        public int VerticalThorttle { get => verticalThorttle; set => verticalThorttle = value; }
        public int LightPwm { get => lightPwm; set => lightPwm = value; }
        public int Ptz { get => ptz; set => ptz = value; }
        public int Steering { get => steering; set => steering = value; }
        public float Ppitch1 { get => Ppitch; set => Ppitch = value; }
        public float Proll1 { get => Proll; set => Proll = value; }
        public float Pyaw1 { get => Pyaw; set => Pyaw = value; }
        public float Ipitch1 { get => Ipitch; set => Ipitch = value; }
        public float Iroll1 { get => Iroll; set => Iroll = value; }
        public float Iyaw1 { get => Iyaw; set => Iyaw = value; }
        public float Dpitch1 { get => Dpitch; set => Dpitch = value; }
        public float Droll1 { get => Droll; set => Droll = value; }
        public float Dyaw1 { get => Dyaw; set => Dyaw = value; }


        public byte[] MoveControlData()
        {
            moveData[0] = 0x02;
            moveData[1] = 0x31; //控制帧标志位

            Array.Copy(BitConverter.GetBytes(forwardThrottle), 0, moveData, 2, 4);
            Array.Copy(BitConverter.GetBytes(transverseThrottle), 0, moveData, 6, 4);
            Array.Copy(BitConverter.GetBytes(verticalThorttle), 0, moveData, 10, 4);
            Array.Copy(BitConverter.GetBytes(lightPwm), 0, moveData, 14, 4);
            Array.Copy(BitConverter.GetBytes(ptz), 0, moveData, 18, 4);
            Array.Copy(BitConverter.GetBytes(steering), 0, moveData, 22, 4);

            moveData[26] = 0x0D;//帧尾
            moveData[27] = 0x0A;

            return moveData;
        }

        public byte[] AdjustData()
        {
            adjustData[0] = 0x02;
            adjustData[1] = 0x32; //控制帧标志位

            Array.Copy(BitConverter.GetBytes(Ppitch), 0, adjustData, 2, 4);
            Array.Copy(BitConverter.GetBytes(Proll), 0, adjustData, 6, 4);
            Array.Copy(BitConverter.GetBytes(Pyaw), 0, adjustData, 10, 4);
            Array.Copy(BitConverter.GetBytes(Ipitch), 0, adjustData, 14, 4);
            Array.Copy(BitConverter.GetBytes(Iroll), 0, adjustData, 18, 4);
            Array.Copy(BitConverter.GetBytes(Iyaw), 0, adjustData, 22, 4);
            Array.Copy(BitConverter.GetBytes(Dpitch), 0, adjustData, 26, 4);
            Array.Copy(BitConverter.GetBytes(Droll), 0, adjustData, 30, 4);
            Array.Copy(BitConverter.GetBytes(Dyaw), 0, adjustData, 34, 4);

            adjustData[38] = 0x0D;//帧尾
            adjustData[39] = 0x0A;

            return adjustData;
        }
        #endregion

    }
}
